from math import sin, cos
from OpenGL.GL import *
from numpy import *

class Matrix3D(object):
	def __init__(self):
		self.m = self.new_matrix()
	
	def new_matrix(self):
		return triu(tri(4,4,k=0),k=0)

	def translateMatrix44(self, x, y, z):
		self.m[3][0] += x
		self.m[3][1] += y
		self.m[3][2] += z
		return self.m

	def scaleMatrix44(self,m, x, y, z):
		self.m[0][0] *= x
		self.m[1][1] *= y
		self.m[2][2] *= z
		return self.m

	def rotationMatrix44(self,angle, x, y, z):
		s = sin(angle)
		c = cos(angle)
		mag = float((x * x + y * y + z * z) ** 0.5)

		if mag == 0.0:
			self.m = self.new_matrix()
			return
		
	  	x /= mag
	  	y /= mag
	  	z /= mag

	 	xx = x * x
	 	yy = y * y
	 	zz = z * z
	 	xy = x * y
	 	yz = y * z
	 	zx = z * x
	 	xs = x * s
	 	ys = y * s
	 	zs = z * s
	 	one_c = 1.0 - c

	 	self.m[0][0] = (one_c * xx) + c
	 	self.m[0][1] = (one_c * xy) - zs
	 	self.m[0][2] = (one_c * zx) + ys
	 	self.m[0][3] = 0.0

	 	self.m[1][0] = (one_c * xy) + zs
	 	self.m[1][1] = (one_c * yy) + c
	 	self.m[1][2] = (one_c * yz) - xs
	 	self.m[1][3] = 0.0

	 	self.m[2][0] = (one_c * zx) - ys
	 	self.m[2][1] = (one_c * yz) + xs
	 	self.m[2][2] = (one_c * zz) + c
	 	self.m[2][3]  = 0.0

	 	self.m[3][0] = 0.0
	 	self.m[3][1] = 0.0
	 	self.m[3][2] = 0.0
	 	self.m[3][3] = 1.0
	 	return self.m

	def matrixMultiply44(self,product, a, b):
		for i in range(0, 4):
			ai0 = a[i]
			ai1 = a[i + (1 << 2)]
			ai2 = a[i + (2 << 2)]
			ai3 = a[i + (3 << 2)]

		 	product[i] = ai0 * b[0] + ai1 * b[1] + ai2 * b[2] + ai3 * b[3]
		 	product[i + (1 << 2)] = ai0 * b[0 + (1 << 2)] + ai1 * b[1 + (1 << 2)] + ai2 * b[2 + (1 << 2)] + ai3 * b[3 + (1 << 2)]
		 	product[i + (2 << 2)] = ai0 * b[0 + (2 << 2)] + ai1 * b[1 + (2 << 2)] + ai2 * b[2 + (2 << 2)] + ai3 * b[3 + (2 << 2)]
		 	product[i + (3 << 2)] = ai0 * b[0 + (3 << 2)] + ai1 * b[1 + (3 << 2)] + ai2 * b[2 + (3 << 2)] + ai3 * b[3 + (3 << 2)]

	# Transpose/Invert - Only 4x4 matricies supported
	def transposeMatrix44(self,dst, src):
		for j in range(0, 4):
			for i in range(0, 4):
				dst[(j * 4) + i] = src[(i * 4) + j]
	
	def invertMatrix44(self,dst, src):
		mat = matrix([[src[0], src[1], src[2], src[3]],
                      [src[4], src[5], src[6], src[7]],
                      [src[8], src[9], src[10], src[11]],
                      [src[12], src[13], src[14], src[15]]])
		mat = mat.I.tolist()               
		dst[0] = mat[0][0]
		dst[1] = mat[0][1]
		dst[2] = mat[0][2]
		dst[3] = mat[0][3]

	 	dst[4] = mat[1][0]
	 	dst[5] = mat[1][1]
	 	dst[6] = mat[1][2]
	 	dst[7] = mat[1][3]

	 	dst[8] = mat[2][0]
	 	dst[9] = mat[2][1]
	 	dst[10] = mat[2][2]
	 	dst[11] = mat[2][3]

	 	dst[12] = mat[3][0]
	 	dst[13] = mat[3][1]
	 	dst[14] = mat[3][2]
	 	dst[15] = mat[3][3]

	



